#define TRUE 1
#define DEBUG_CHESSBOARD

#include <iostream>
#include <vector>
#include <ctime>

#include "augmentOpencv.h"
#include "learningOpenCVHeader.h"
#include "DataReader.h"
#include "KinectWrapper.h"

using namespace std;
using namespace cv;

char *imgName, *videoName, *img2Name;
IplImage* gImg;
double thrAlpha = 0.12;
double sig = 0.00001;
double eps = 0.00001;
int winSize = 1;

void showImage() {
    IplImage* img = cvLoadImage(imgName);
    cvNamedWindow("Example1", CV_WINDOW_AUTOSIZE);
    cvShowImage("Example1", img);
    cvReleaseImage(&img);
	CvMat *imgM = cvLoadImageM(imgName);
	cvShowImage("Example1", imgM);
	CvMat *imgG = cvCreateMat(imgM->rows, imgM->cols, CV_32F);
	cvCvtColor(imgM, imgG, CV_BGR2GRAY);
	cvShowImage("gray", imgG);
}

void showVideo() {
    cvNamedWindow("video test", CV_WINDOW_AUTOSIZE);
    CvCapture* capture = cvCreateFileCapture(videoName);
    IplImage* frame;
    while (true) {
        frame = cvQueryFrame(capture);
        if (!frame) break;
        cvShowImage("video test", frame);
        char c = cvWaitKey(33);
        if (c == 27) break;
    }
    cvReleaseCapture(&capture);
//    cvDestroyWindow("video test");
}

void showSmooth() {
    cvNamedWindow("smooth", CV_WINDOW_AUTOSIZE);
    IplImage* img = cvLoadImage(imgName);
    IplImage* outimg = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
    cvSmooth(img, outimg, CV_GAUSSIAN, 3, 3);
    cvShowImage("smooth", outimg);
    cvReleaseImage(&img);
    cvReleaseImage(&outimg);
//    cvWaitKey(0);
//    cvDestroyWindow("smooth");
//    cvDestroyWindow("result");
}

void showHalf() {
    cvNamedWindow("half", CV_WINDOW_AUTOSIZE);
    IplImage *img = cvLoadImage(imgName);
    IplImage *outimg = cvCreateImage(cvSize(img->width/2, img->height/2), img->depth, img->nChannels);
    cvPyrDown(img, outimg);
    cvShowImage("half", outimg);
    cvReleaseImage(&img);
    cvReleaseImage(&outimg);
//    cvWaitKey(0);
}

void showCanny() {
    cvNamedWindow("canny", CV_WINDOW_AUTOSIZE);
    IplImage *img = cvLoadImage(imgName, CV_LOAD_IMAGE_GRAYSCALE);
    IplImage *outimg = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
    cvCanny(img, outimg, 10, 50, 3);
    cvShowImage("canny", outimg);
    cvReleaseImage(&img);
    cvReleaseImage(&outimg);
}

void useMat() {
    double vals[] = {0.1, 0.2, 0.3, 0.4};
    CvMat rotmat;
    cvInitMatHeader(&rotmat, 2, 2, CV_64FC1, vals);

    unsigned char vals2[] = {11,12,13,14,15,16,17,18,19,21,22,23,24,25,26,27,28,29,31,32,33,34,35,36,37,38,39};
    CvMat *mat = cvCreateMat(3, 3, CV_8UC3);
	cout << cvGet2D(mat, 1, 1).val[0] << " " 
		<< cvGet2D(mat, 1, 1).val[1] << " "
		<< cvGet2D(mat, 1, 1).val[2] << " "
		<< cvGet2D(mat, 1, 1).val[3] << endl;
    mat->data.ptr = vals2;
    cout << (int)CV_MAT_ELEM(*mat, unsigned char, 1, 0) << endl;
    //vals2 actually is 3D
    cout << cvGet2D(mat, 1, 1).val[0] << " " 
            << cvGet2D(mat, 1, 1).val[1] << " "
            << cvGet2D(mat, 1, 1).val[2] << " "
            << cvGet2D(mat, 1, 1).val[3] << endl;
    cout << cvGetReal2D(&rotmat, 1, 1) << endl;

    //try get dims, use CV_MAX_DIM
    int *dims = new int[CV_MAX_DIM];
    int dim;
    cout << (dim = cvGetDims(mat, dims)) << endl;
    for (int i=0; i<dim; ++i)
        cout << dims[i] << endl;

    //try cvReduce
    CvMat *reduced = cvCloneMat(mat);
    cvReduce(mat, reduced, 0, CV_REDUCE_SUM);

}

void showROI() {
    cvNamedWindow("ROI", CV_WINDOW_AUTOSIZE);
    IplImage *img = cvLoadImage(imgName);
    cvSetImageROI(img, cvRect(100, 100, 50, 50));
    cvAddS(img, cvScalar(0, 0, 150), img);
    cvResetImageROI(img);
    cvShowImage("ROI", img);
    cvReleaseImage(&img);
}

void showAddWeight() {
    IplImage *girl = cvLoadImage("11.jpg");
    IplImage *boy = cvLoadImage("12.jpg");
    IplImage *newboy = cvCreateImage(cvSize(boy->width/2, boy->height/2), boy->depth, boy->nChannels);
    IplImage *mix = cvCreateImage(cvSize(500,800), boy->depth, boy->nChannels);
    cvPyrDown(boy, newboy);
    CvRect finalFace = cvRect(600, 100, 500, 800);
    cvSetImageROI(girl, cvRect(600, 200, 500, 800));
    cvSetImageROI(newboy, cvRect(500, 100, 500, 800));
    cvNamedWindow("girl");
    cvNamedWindow("boy");
    cvShowImage("girl", girl);
    cvShowImage("boy", newboy);
    cvAddWeighted(girl, 0.3, newboy, 0.7, 30, mix);
    cvNamedWindow("mix");
    cvShowImage("mix", mix);
    cvReleaseImage(&girl);
    cvReleaseImage(&boy);
    cvReleaseImage(&newboy);
    cvReleaseImage(&mix);
}

void drawImage() {
    IplImage *canvas = cvCreateImage(cvSize(1000, 1000), 8, 3);

    cvLine(canvas, cvPoint(100, 100), cvPoint(200, 400), CV_RGB(255, 127, 63), 10, 8, 0);
    cvRectangle(canvas, cvPoint(800, 800), cvPoint(600, 400), CV_RGB(63,127,255), 5);
    cvCircle(canvas, cvPoint(500, 500), 50, CV_RGB(127, 255, 63), CV_FILLED);
    cvEllipse(canvas, cvPoint(500, 300), cvSize(200, 90), 30, 0, 360, CV_RGB(255, 63, 127), 3);
    CvBox2D box;
    box.center = cvPoint2D32f(200, 700);
    box.angle = -30;
    box.size = cvSize2D32f(120, 160);
    cvEllipseBox(canvas, box, CV_RGB(63, 255, 127), 4);
    CvPoint** pts = new CvPoint*[5];
    CvPoint* pts2 = new CvPoint[5];
    pts2[0] = cvPoint(800, 20);
    pts2[1] = cvPoint(700, 100);
    pts2[2] = cvPoint(600, 60);
    pts2[3] = cvPoint(700, 30);
    pts2[4] = cvPoint(900, 40);
    for (int i=0; i<5; ++i) pts[i] = &pts2[i];
    int* npts = new int[2];
    npts[0] = 5;
    npts[1] = 2;
    cvFillPoly(canvas, pts, npts, 1, CV_RGB(127, 63, 255));
    for (int i=0; i<5; ++i) {
        pts2[i].x += 50;
        pts2[i].y += 110;
    }
    cvFillConvexPoly(canvas, pts2, 5, CV_RGB(127, 63, 255));
    for (int i=0; i<5; ++i) {
        pts2[i].x += 50;
        pts2[i].y += 110;
    }
    cvPolyLine(canvas, pts, npts, 1 ,TRUE, CV_RGB(255, 128, 128), 6);

    CvFont font;
    cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_SIMPLEX, 2, 2, 0.1, 2, 8);
    cvPutText(canvas, "hi guy", cvPoint(300, 50), &font, CV_RGB(128, 255, 128));


    cvNamedWindow("draw", CV_WINDOW_AUTOSIZE);
    cvShowImage("draw", canvas);
    cvReleaseImage(&canvas);
}

void useXML() {
    CvFileStorage *fs = cvOpenFileStorage("trytry.xml", 0, CV_STORAGE_WRITE);
    cvWriteInt(fs, "No", 123456789);
    cvStartWriteStruct(fs, "school", CV_NODE_MAP);
    cvStartWriteStruct(fs, "xiaoxue", CV_NODE_SEQ);
    cvWriteString(fs, 0, "jiangkou", true);
    cvEndWriteStruct(fs);
    cvStartWriteStruct(fs, "zhongxue", CV_NODE_SEQ);
    cvWriteString(fs, 0, "qizhong");
    cvEndWriteStruct(fs);
    cvStartWriteStruct(fs, "gaozhong", CV_NODE_SEQ);
    cvWriteString(fs, NULL, "nanshan", TRUE);
    cvEndWriteStruct(fs);
    cvStartWriteStruct(fs, "daxue", CV_NODE_SEQ);
    cvWriteString(fs, NULL, "jiaoda");
    cvEndWriteStruct(fs);
    cvEndWriteStruct(fs);
    CvMat *mat = cvCreateMat(3, 3, CV_8UC3);
    cout << cvGet2D(mat, 1, 1).val[0] << endl;
    cvWrite(fs, "random_mat", mat);
    cvReleaseFileStorage(&fs);

    fs = cvOpenFileStorage("trytry.xml", 0, CV_STORAGE_READ);

    CvFileNode* queue[100];
    int qin, qout;
    qin = qout = 0;
    CvFileNode *root = cvGetRootFileNode(fs);
    queue[qin++] = root;

    cout << CV_NODE_TYPE(root->tag) << endl;

    while (qin > qout) {
        CvFileNode *cur, *next;
        cur = queue[qout++];
        cout << cvGetFileNodeName(cur) << endl;
        if (CV_NODE_IS_COLLECTION(cur->tag)) {
            CvSeq* s = cur->data.seq;
            CvFileNode *last = (CvFileNode*)cvGetSeqElem(s, -1);
            int i=0;
            do 
            {
                next = (CvFileNode*)cvGetSeqElem(s, i++);
                queue[qin++] = next;
            } while (last != next);
        }
    }

    int no = cvReadIntByName(fs, 0, "No");
    cout << no << endl;

    CvFileNode *school = cvGetFileNodeByName(fs, 0, "school");
    cout << cvGetFileNodeName(school) << endl;
    cout << cvReadStringByName(fs, school, "daxue") << endl;
    

    CvSeq* s = cvGetFileNodeByName(fs, 0, "school")->data.seq;
    cout << cvReadString((CvFileNode*)cvGetSeqElem(s, 0)) << endl;
    cout << cvReadString((CvFileNode*)cvGetSeqElem(s, 1)) << endl;
    cout << cvReadString((CvFileNode*)cvGetSeqElem(s, 2)) << endl;
    cout << cvReadString((CvFileNode*)cvGetSeqElem(s, 3)) << endl;
    CvMat* matrix = (CvMat*) cvReadByName(fs, 0, "random_mat");
    cout << cvGet2D(matrix, 1, 1).val[0] << endl;
    cvReleaseFileStorage(&fs);

}

void checkIPP(void) {
    char *libs, *mods;
    cvGetModuleInfo(0, (const char **)&libs, (const char **)&mods);
    cout << "lib: " << libs << endl
            << "mod: " << mods << endl;
}

void drawRectangle(int type, int x, int y, int flag, void* param) {
    static CvPoint first, second;
    switch (type) {
        case CV_EVENT_LBUTTONDOWN:
            first = cvPoint(x, y);
            break;
        case CV_EVENT_LBUTTONUP:
            {
                second = cvPoint(x, y);
                IplImage* canvas = (IplImage*) param;
                CvFont font;
                cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1, 1);
                char words[100];
                sprintf_s(words, 100, "first=(%d, %d), second=(%d, %d)", first.x, first.y, second.x, second.y);
                cvSet(canvas, CV_RGB(255,255,255));
                cvPutText(canvas, words, cvPoint(50,50), &font, CV_RGB(0,0,0));
                cvRectangle(canvas, first, second, CV_RGB(127,63,31), -1);
                cvShowImage("mouse", canvas);
            }
            break;
        case CV_EVENT_MOUSEMOVE:
            if (flag & CV_EVENT_FLAG_LBUTTON) {
                second = cvPoint(x, y);
                IplImage* canvas = (IplImage*) param;
                CvFont font;
                cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
                char words[100];
                sprintf_s(words, 100, "first=(%d, %d), second=(%d, %d)", first.x, first.y, second.x, second.y);
                cvSet(canvas, CV_RGB(255,255,255));
                cvPutText(canvas, words, cvPoint(50,50), &font, CV_RGB(0,0,0));
                cvRectangle(canvas, first, second, CV_RGB(127,63,31));
                cvShowImage("mouse", canvas);
            }
            break;
        default:
            break;
    }

}

void tryMouseEvent() {
    cvNamedWindow("mouse");
    IplImage *canvas = cvCreateImage(cvSize(1200, 600), 8, 3);
    cvSetMouseCallback("mouse", drawRectangle, canvas);
    cvSet(canvas, CV_RGB(255,255,255));
    cvShowImage("mouse", canvas);
}

void updownImg(int pos) {
    cvConvertImage(gImg, gImg, CV_CVTIMG_FLIP);
    cvShowImage("trackbar2", gImg);
}

void tryTrackbar() {
    cvNamedWindow("trackbar");
    IplImage *img = cvLoadImage(imgName);
    IplImage *empty = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    int state = 0;
    while (1) {
        cvCreateTrackbar("switch", "trackbar", &state, 1, NULL);
        if (state)
            cvShowImage("trackbar", img);
        else
            cvShowImage("trackbar", empty);
        if (cvWaitKey(100) != -1) break;
    }

    gImg = img;
    cvNamedWindow("trackbar2");
    cvCreateTrackbar("switch", "trackbar2", &state, 1, updownImg);
    cvShowImage("trackbar2", img);
}

void tryBlur() {
    IplImage *img = cvLoadImage(imgName);
    cvNamedWindow("original");
    cvShowImage("original", img);

    IplImage *smooth1, *smooth2, *smooth3, *smooth4, *smooth5;
    smooth1 = (IplImage*) cvClone(img);
    smooth2 = (IplImage*) cvClone(img);
    smooth3 = cvCreateImage(cvGetSize(img), IPL_DEPTH_16S, 3);
    smooth4 = (IplImage*) cvClone(img);
    smooth5 = (IplImage*) cvClone(img);
    
    cvSmooth(img, smooth1, CV_GAUSSIAN, 5, 1, 0, 0);
    cvSmooth(img, smooth2, CV_BLUR, 5, 1, 0, 0);
    cvSmooth(img, smooth3, CV_BLUR_NO_SCALE, 9, 1, 0, 0);
    cvSmooth(img, smooth4, CV_MEDIAN, 5, 1, 0, 0);
    cvSmooth(img, smooth5, CV_BILATERAL, 16, 5, 0, 0);
    cvNamedWindow("gaussian");
    cvShowImage("gaussian", smooth1);
    cvNamedWindow("blur");
    cvShowImage("blur", smooth2);
    cvNamedWindow("noscale");
    cvShowImage("noscale", smooth3);
    cvNamedWindow("median");
    cvShowImage("median", smooth4);
    cvNamedWindow("bilateral");
    cvShowImage("bilateral", smooth5);
}

void tryMorphology() {
    IplImage *img = cvLoadImage(imgName);
    cvNamedWindow("orginal");
    cvShowImage("orginal", img);

    int values[] = {0,0,0,0,0,0,0,0,0,10000};
    IplConvKernel* kernel = cvCreateStructuringElementEx(10, 10, 5, 5, CV_SHAPE_RECT);

    IplImage *erode = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    cvErode(img, erode, kernel);
    cvNamedWindow("erode");
    cvShowImage("erode", erode);

    IplImage *dilate = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    cvDilate(img, dilate, kernel);
    cvNamedWindow("dilate");
    cvShowImage("dilate", dilate);

    IplImage *dilate2 = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    cvDilate(erode, dilate2, kernel);
    cvNamedWindow("dilate2");
    cvShowImage("dilate2", dilate2);

    IplImage *erode2 = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    cvErode(dilate, erode2, kernel);
    cvNamedWindow("erode2");
    cvShowImage("erode2", erode2);


    IplImage *dst1 = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    IplImage *dst2 = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    IplImage *tmp = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    cvMorphologyEx(img, dst1, tmp, kernel, CV_MOP_TOPHAT);
    cvMorphologyEx(img, dst2, tmp, kernel, CV_MOP_BLACKHAT);
    cvNamedWindow("TopHat");
    cvShowImage("TopHat", dst1);
    cvNamedWindow("BlackHat");
    cvShowImage("BlackHat", dst2);

    cvReleaseStructuringElement(&kernel);
}

void tryFloodfill()
{
    IplImage *img = cvLoadImage(imgName);
    cvNamedWindow("ori");
    cvShowImage("ori", img);

    int flag = 0;
    flag = CV_FLOODFILL_FIXED_RANGE | CV_FLOODFILL_MASK_ONLY | 255 << 8;
    IplImage *mask = cvCreateImage(cvSize(img->width+2, img->height+2), IPL_DEPTH_8U, 1);
    cvSetZero(mask);

    cvNamedWindow("mask_1");
    cvShowImage("mask_1", mask);
    cvFloodFill(img, cvPoint(100, 100), CV_RGB(120, 60, 90), CV_RGB(20,20,20), CV_RGB(20,20,20), 
            NULL, flag, mask);
    cvNamedWindow("mask_2");
    cvShowImage("mask_2", mask);
    cvNamedWindow("floodfill");
    cvShowImage("floodfill", img);
}

void tryPyramid() {
    IplImage *img = cvLoadImage(imgName);
    cvNamedWindow("first");
//    cvSmooth(img, img, CV_GAUSSIAN);
    cvShowImage("first", img);

    IplImage *img2 = cvCreateImage(cvSize(img->width/2, img->height/2), img->depth, img->nChannels);
    cvPyrDown(img, img2);
    cvNamedWindow("second");
    cvShowImage("second", img2);

    IplImage *img3 = cvCreateImage(cvSize(img2->width/2, img2->height/2), img2->depth, img2->nChannels);
    cvPyrDown(img2, img3);
    cvNamedWindow("third");
    cvShowImage("third", img3);

    IplImage *img4 = cvCreateImage(cvSize(img2->width*2, img2->height*2), img2->depth, img2->nChannels);
    cvPyrUp(img2, img4);
    cvNamedWindow("four");
    cvShowImage("four", img4);

    cout << img->width << " " << img->height << "   " << img4->width << " " << img4->height << endl;
}

void trySegmentation() {
    IplImage *img = cvLoadImage(imgName);
    cvNamedWindow("ori");
    cvShowImage("ori", img);

    CvMemStorage *memStorage = cvCreateMemStorage();
    CvSeq *seq = NULL;
    cout << "img width=" << img->width << "   img height=" << img->height << endl;
    int lev = 3;
    IplImage *resizeImg = cvCreateImage(
            cvSize(img->width >> lev << lev, img->height >> lev << lev), 
            img->depth, img->nChannels);
    IplImage *tmpImg = (IplImage *) cvClone(resizeImg);
    cvResize(img, resizeImg);
    cout << "resize width=" << resizeImg->width << "   resize height=" << resizeImg->height << endl;
    cvPyrSegmentation(resizeImg, tmpImg, memStorage, &seq, lev, 150, 30);
    cout << seq->total << endl;
    cvNamedWindow("tmp");
    cvShowImage("tmp", tmpImg);
}

void tryHist() {
	// Set up images
	IplImage* img = cvLoadImage(imgName);
	IplImage* back_img = cvCreateImage( cvGetSize( img ), IPL_DEPTH_8U, 1 );

	// Compute HSV image and separate into colors
	IplImage* hsv = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 3 );
	cvCvtColor( img, hsv, CV_BGR2HSV );

	IplImage* h_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* s_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* v_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* planes[] = { h_plane, s_plane };
	cvCvtPixToPlane( hsv, h_plane, s_plane, v_plane, 0 );

	// Build and fill the histogram
	int h_bins = 30, s_bins = 32;
	CvHistogram* hist;
	{
		int hist_size[] = { h_bins, s_bins };
		float h_ranges[] = { 0, 180 };
		float s_ranges[] = { 0, 255 };
		float* ranges[] = { h_ranges, s_ranges };
		hist = cvCreateHist( 2, hist_size, CV_HIST_ARRAY, ranges, 1 );
	}
	cvCalcHist( planes, hist, 0, 0 ); // Compute histogram
	cout << *cvGetHistValue_2D(hist, 9, 30) << endl;
	cvNormalizeHist( hist, 10*255 ); // Normalize it
	cout << *cvGetHistValue_2D(hist, 9, 30) << endl;
	cvNormalizeHist( hist, 15.7*255 ); // Normalize it
	cout << *cvGetHistValue_2D(hist, 9, 30) << endl;


	cvCalcBackProject( planes, back_img, hist );// Calculate back projection
	cvNormalizeHist( hist, 1.0 ); // Normalize it


	// Create an image to visualize the histogram
	int scale = 10;
	IplImage* hist_img = cvCreateImage( cvSize( h_bins * scale, s_bins * scale ), 8, 3 );
	cvZero ( hist_img );

	// populate the visualization
	float max_value = 0;
	cvGetMinMaxHistValue( hist, 0, &max_value, 0, 0 );

	for( int h = 0; h < h_bins; h++ ){
		for( int s = 0; s < s_bins; s++ ){
			float bin_val = cvQueryHistValue_2D( hist, h, s );
			int intensity = cvRound( bin_val * 255 / max_value );
			cvRectangle( hist_img, cvPoint( h*scale, s*scale ),
				cvPoint( (h+1)*scale - 1, (s+1)*scale - 1 ),
				CV_RGB( intensity, intensity, intensity ),
				CV_FILLED );
		}
	}

	// Show original
	cvNamedWindow( "Source", 1) ;
	cvShowImage( "Source", img );

	// Show back projection
	cvNamedWindow( "Back Projection", 1) ;
	cvShowImage( "Back Projection", back_img );

	// Show histogram equalized
	cvNamedWindow( "H-S Histogram", 1) ;
	cvShowImage( "H-S Histogram", hist_img );
}

IplImage *readDoubleImage(char* name) {
    IplImage *tmp, *final;
    tmp = cvLoadImage(name);
    final = cvCreateImage(cvGetSize(tmp), IPL_DEPTH_32F, 3);
    cvConvertScale(tmp, final, 1.0/255);
    cvReleaseImage(&tmp);
    return final;
}

IplImage *upSmpIm(IplImage *img, CvSize size) {
    IplImage *resImg;
    resImg = cvCreateImage(size, img->depth, img->nChannels);
    cvPyrUp(img, resImg);
    return resImg;
}

IplImage *flatImg(IplImage *img) {
    return NULL;
}

CvMat *img2Mat(IplImage *img) {
    CvMat *mat = NULL;
    switch (img->depth) {
        case IPL_DEPTH_64F: 
            {
            mat = cvCreateMat(img->height, img->width, CV_64FC(img->nChannels));
            double *matPtr;
            double *imgPtr;
            matPtr = mat->data.db;
            imgPtr = (double*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_32F: 
            {
            mat = cvCreateMat(img->height, img->width, CV_32FC(img->nChannels));
            float *matPtr;
            float *imgPtr;
            matPtr = mat->data.fl;
            imgPtr = (float*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_32S: 
            {
            mat = cvCreateMat(img->height, img->width, CV_32SC(img->nChannels));
            int *matPtr;
            int *imgPtr;
            matPtr = mat->data.i;
            imgPtr = (int*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_16U: 
            {
            mat = cvCreateMat(img->height, img->width, CV_16UC(img->nChannels));
            ushort *matPtr;
            ushort *imgPtr;
            matPtr = (ushort*)(mat->data.s);
            imgPtr = (ushort*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_16S:
            {
            mat = cvCreateMat(img->height, img->width, CV_16SC(img->nChannels));
            short *matPtr;
            short *imgPtr;
            matPtr = mat->data.s;
            imgPtr = (short*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_8U:
            {
            mat = cvCreateMat(img->height, img->width, CV_8UC(img->nChannels));
            uchar *matPtr;
            uchar *imgPtr;
            matPtr = mat->data.ptr;
            imgPtr = (uchar*) (img->imageData);
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        case IPL_DEPTH_8S:
            {
            mat = cvCreateMat(img->height, img->width, CV_8SC(img->nChannels));
            char *matPtr;
            char *imgPtr;
            matPtr = (char*) (mat->data.ptr);
            imgPtr = img->imageData;
            for (int i=0; i<img->height; ++i)
                for (int j=0; j<img->width; ++j) {
                    *matPtr = *imgPtr;
                    ++matPtr;
                    ++imgPtr;
                }
            }
            break;
        default:
            cout << "no such type img.[in img2Mat()]" << endl;
    }

    return mat;
}

IplImage *getLinearCoeff(IplImage *sAlpha, IplImage *I, double eps, int winSize) {
    if (eps < 0) eps = 0.0000001;
    if (winSize < 0) winSize = 1;
    int nebSize = (winSize * 2 + 1) * (winSize * 2 + 1);
    int h = I->height, w = I->width, c = I->nChannels;
    int n = h, m = w;
    int imgSize = w * h;
    int **indsM;
    indsM = new int*[h];
    for (int i=0; i<h; ++i)
        indsM[i] = new int[w];

    int tmp = 0;
    for (int i=0; i<h; ++i)
        for (int j=0; j<w; ++j)
            indsM[i][j] = ++tmp;
    double ***coeff;
    //[h][w][c+1];
    coeff = new double**[h];
    for (int i=0; i<h; ++i) {
        coeff[i] = new double *[w];
        for (int j=0; j<w; ++j)
            coeff[i][j] = new double[c+1];
    }

    memset(coeff, 0, sizeof(double) * h * w * (c+1));
    int *winInds;
    winInds = new int[(2*winSize + 1)*(2*winSize + 1)];
    for (int j=1+winSize; j<=w-winSize; ++j)
        for (int i=winSize+1; i<=h-winSize; ++i) {
            tmp = 0;
            for (int k=i-winSize; k<=i+winSize; ++k)
                for (int l=j-winSize; l<=j+winSize; ++l)
                    winInds[tmp++] = indsM[k][l];
            flatImg(I);


        }

    return NULL;
}

IplImage *upSampleAlphaUsingImg(IplImage *sAlpha, IplImage *I, IplImage *bI, double eps, int winSize) {
    IplImage *coeff, *bcoeff;

    coeff = getLinearCoeff(sAlpha, I, eps, winSize);

    bcoeff = upSmpIm(sAlpha, cvGetSize(bI));

    //balpha=bcoeff(:,:,end);

    //for k=1:size(bI,3)
    //    balpha=balpha+bcoeff(:,:,k).*bI(:,:,k);
    return NULL;
}

IplImage *downSmpIm(IplImage *img, int scale) {
    IplImage *resImg;
    resImg = cvCreateImage(cvSize(img->width / scale, img->height / scale), img->depth, img->nChannels);
    cvPyrDown(img, resImg);
    return resImg;
}

IplImage *solveAlphaC2F(IplImage *I, IplImage *consts_map, IplImage *consts_val, int lvNum, int activeLvNum, double thrAlpha, double eps, int winSize) {
    if (thrAlpha < 0)
        thrAlpha = 0.02;
    int erode_mask_w = 1;
    activeLvNum = max(activeLvNum, 1);
    if (lvNum > 1) {
        IplImage *sI = downSmpIm(I, 2);
        IplImage *s_consts_map, *s_consts_val;
        s_consts_map = downSmpIm(consts_map, 2);
        s_consts_val = downSmpIm(consts_val, 2);
        IplImage *sAlpha = solveAlphaC2F(sI, s_consts_map, s_consts_val, lvNum-1, 
            min(lvNum-1, activeLvNum), thrAlpha, eps, winSize);
        IplImage *oAlpha = upSampleAlphaUsingImg(sAlpha, sI, I, eps, winSize);


    }
    return NULL;
}

void runningMatting()
{
    int lvNum, activeLvNum;
    lvNum = 1;
    activeLvNum = 1;

    IplImage *I, *mI;

    I = readDoubleImage(imgName);
    cout << I->height << " " << I->width << endl;

    cvNamedWindow("ori");
    cvShowImage("ori", I);

    //get mask image
    mI = readDoubleImage(img2Name);
    cvNamedWindow("mask");
    cvShowImage("mask", mI);
    
    IplImage *res = cvCloneImage(I);
    cvSetZero(res);
    cvSub(I, mI, res);
    IplImage *rc, *gc, *bc, *sumc;
    rc = cvCreateImage(cvSize(res->width, res->height), IPL_DEPTH_32F, 1);
    gc = cvCreateImage(cvSize(res->width, res->height), IPL_DEPTH_32F, 1);
    bc = cvCreateImage(cvSize(res->width, res->height), IPL_DEPTH_32F, 1);
    sumc = cvCreateImage(cvSize(res->width, res->height), IPL_DEPTH_32F, 1);
    cvSplit(res, rc, gc, bc, NULL);
    cvReleaseImage(&res);

    cvAdd(rc, gc, sumc);
    cvAdd(sumc, bc, sumc);
    cvReleaseImage(&rc);
    cvReleaseImage(&gc);
    cvReleaseImage(&bc);

    IplImage *consts_map;
    consts_map = cvCreateImage(cvGetSize(sumc), IPL_DEPTH_8U, 1);
    cvCmpS(sumc, 0.001, consts_map, CV_CMP_GT);
    cvReleaseImage(&sumc);

    IplImage *consts_val;
    consts_val = cvCreateImage(cvGetSize(consts_map), IPL_DEPTH_32F, 1);
    IplImage *graymI;
    graymI = cvCreateImage(cvGetSize(consts_map), IPL_DEPTH_32F, 1);
    cvCvtColor(mI, graymI, CV_RGB2GRAY);
    IplImage *anti_consts_map;
    anti_consts_map = cvCloneImage(consts_map);
    cvSet(anti_consts_map, cvScalar(1));
    cvSet(anti_consts_map, cvScalar(0), consts_map);
    cvSet(graymI, cvScalar(0), anti_consts_map);

    solveAlphaC2F(I, consts_map, consts_val, lvNum, activeLvNum, thrAlpha, -1, -1);
}

void oldLearning(void) {
//    showImage();
//    showVideo();
//    showSmooth();
//    showHalf();
//    showCanny();

//    useMat();
//    showROI();
//    showAddWeight();
//    drawImage();
//    useXML();x
//    checkIPP();
//    tryMouseEvent();
//    tryTrackbar();
//    tryBlur();
//    tryMorphology();
//    tryFloodfill();
//    tryPyramid();
//    trySegmentation();
//    tryHist();
}

void tryHarris(void) {
	CvMat *srcMat = cvLoadImageM(imgName);
	CvMat *desMat = cvCreateMat(srcMat->rows, srcMat->cols, CV_32F);
	CvMat *oneCh = cvCreateMat(srcMat->rows, srcMat->cols, CV_8U);
	oneCh = cvRgb2gray(srcMat);

  	cvPreCornerDetect(oneCh, desMat);
	cvNamedWindow("pre");
	cvShowImage("pre", desMat);

	CvMat *corner_mask = cvCreateMat( desMat->rows, desMat->cols, CV_8U);
	cvScale(desMat, desMat, 100);
	cvPrintMat(desMat, "desMat.txt");
 	cvCmpS(desMat, 0, corner_mask, CV_CMP_GT );
	cvNamedWindow("mask");
	cvShowImage("mask", corner_mask);
	cvPrintMat(corner_mask, "mask.txt");

	cvCornerHarris(oneCh, desMat, 5, 5);

	CvMat *desMat8U = cvConvertMatType(desMat, CV_8U);
	cvThreshold(desMat, desMat8U, 0, 255, CV_THRESH_BINARY);
	cvNamedWindow("desMat8U");
	cvShowImage("desMat8U", desMat8U);
	cvAnd(desMat8U, corner_mask, desMat8U);

	cvNamedWindow("desMat8Umask");
	cvShowImage("desMat8Umask", desMat8U);
	cvPrintMat(desMat8U, "desMat8u.txt");
	
	CvPointListList pll = cvBwlabel(desMat8U);

	double minval, maxval;
	int minidx, maxidx;
	
	cvZero(desMat8U);
	for (int i=0; i<pll.length; ++i) {
		cvMinMaxLoc(desMat, *pll.lists[i], minval, maxval, minidx, maxidx);
		cout << pll.lists[i]->data[maxidx].x << " " << pll.lists[i]->data[maxidx].y << endl;
		CvPoint p = pll.lists[i]->data[maxidx];
		swap(p.x, p.y);
		cvDrawCircle(srcMat, p, 1, cvScalar(0,0,255));
		cvSet2D(desMat8U, pll.lists[i]->data[maxidx].x, pll.lists[i]->data[maxidx].y, cvScalar(255));
	}

	cvNamedWindow("srcImg");
	cvShowImage("srcImg", oneCh);
	cvNamedWindow("harris");
	cvShowImage("harris", desMat8U);
	cvNamedWindow("final");
	cvShowImage("final", srcMat);

// 	CvMat *colorImg = cvCloneMat(srcMat);
// 	cvSubRS(desMat8U, cvScalar(255), desMat8U);
// 	cvShowImage("harris", desMat8U);
// 	cvMerge(oneCh, desMat8U, oneCh, NULL, colorImg);
// 	cvNamedWindow("color");
// 	cvShowImage("color", colorImg);
}

void tryFilter(void) {
	CvMat* img = cvLoadImageM("zhang2.jpg");
	CvMat* dst = cvCloneMat(img);
	CvMat* mask = cvCreateMatHeader(3, 3, CV_32F);
	float maskData[] = {0,-1,0,-1,4,-1,0,-1,0};
	mask->data.fl = maskData;

	cvFilter2D(img, dst, mask);
	cvNamedWindow("org");
	cvShowImage("org", img);
	cvNamedWindow("dest");
	cvShowImage("dest", dst);
}

void tryVideo(void) {
	CvCapture* capture = cvCaptureFromCAM(-1); // capture from video device #0
	cvQueryFrame(capture);

	int fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
	cvNamedWindow("infra-red");

	while (true) {
		IplImage* frame = cvQueryFrame(capture);
		cvShowImage("infra-red", frame);
		char c = cvWaitKey(1000/15);
		if (c == 27) break;
	}

	cvReleaseCapture(&capture);
}

void tryLowerSat(CvMat* img = NULL) {
	bool releaseImg = false;
	if (img == NULL) {
		img = cvLoadImageM(imgName);
		releaseImg = true;
	}

	long start_time = clock();

	CvMat* dbImg = cvCreateMat(img->rows, img->cols, CV_32FC3);
	cvConvert(img, dbImg);
	CvMat* hsvImg = cvCloneMat(dbImg);
	cvCvtColor(dbImg, hsvImg, CV_BGR2HSV);

	CvMat* sat = cvCreateMat(img->rows, img->cols, CV_32F);
	CvMat* hue = cvCloneMat(sat);
	CvMat* brt = cvCloneMat(sat);
	cvSplit(hsvImg, hue, sat, brt, NULL);
	cvScale(sat, sat, 0.3);
	cvMerge(hue, sat, brt, NULL, hsvImg);

	cvCvtColor(hsvImg, dbImg, CV_HSV2BGR);
	cvConvert(dbImg, img);

	long end_time = clock();

	//	cout << (end_time - start_time + 0.0) / CLOCKS_PER_SEC << endl;

	cvNamedWindow("infra");
	cvShowImage("infra", img);
	if (releaseImg) cvReleaseMat(&img);
	cvReleaseMat(&dbImg);
	cvReleaseMat(&hsvImg);
	cvReleaseMat(&sat);
	cvReleaseMat(&hue);
	cvReleaseMat(&brt);
}

void testingCode(void) {
	CvMat* img = cvLoadImageM("frame0.jpg");
	cvNamedWindow("debug");
	cvShowImage("debug", img);

	// 	cvPrintMat(img, "liu1.txt");
	// 	cvSetMouseCallback("debug", debugByMouseCh, img);

}

void tryVideoWriter(void) {
	CvCapture* capture = cvCaptureFromCAM(-1); // capture from video device #0
	//	CvCapture* capture = cvCaptureFromFile("test.wmv");
	if (capture == 0) {
		cout << "error" << endl;
		return;
	}
	IplImage* img = 0;

	cvNamedWindow("infra-red");

	IplImage* frame;
	int res = cvGrabFrame(capture);

	frame = cvRetrieveFrame(capture);
	cvShowImage("infra-red", frame);
	char c = cvWaitKey(0);
	if (c == 27) exit(0);


	CvVideoWriter *writer = 0;
	double fps     = 30;//cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);  // or 30
	int frameW  = frame->width; // 744 for firewire cameras
	int frameH  = frame->height; // 480 for firewire cameras

	long t = clock();
	char name[100];
	memset(name, 0, 100);
	sprintf(name, "%d_IRrec.avi", t);
	writer=cvCreateVideoWriter(name, -1, fps, cvSize(frameW,frameH));

	int cnt = 0;
	while (true) {
		int suc = cvGrabFrame(capture);
		if (suc != 1) break;
		IplImage* frame = cvRetrieveFrame(capture);

		// 		if (cnt >= 400) {
		// 			CvMat* header = cvCreateMatHeader(frame->height,frame->width, CV_8UC3);
		// 			CvMat* img = cvGetMat(frame, header);
		// 			tryLowerSat(img);
		// 			cvReleaseMatHeader(&header);
		// 
		// 			cvShowImage("infra-red", frame);
		// 			cvWriteFrame(writer, frame);      // add the frame to the file
		// 		}
		// 
		// 		printf("processing...%d/910\n", ++cnt);
		cvShowImage("infra-red", frame);
		cvWriteFrame(writer, frame);
		char c = cvWaitKey(1000/30);
		if (c == 27) break;
	}

	cvReleaseVideoWriter(&writer);
	cvReleaseCapture(&capture);
}

void simpleTest(void) {
	CvMat *src = cvLoadImageM("depth.png");
	cvNamedWindow("test");
	cvShowImage("test", src);
	cvPrintMat(src, "depth.txt");
}

void tryCascade(void) {
	CvHaarClassifierCascade *pCascade = 0;
	CvMemStorage *pStorage = 0;
	CvSeq *pFaceRectSeq = 0;
	IplImage *bigImg, *img;
	bigImg = cvLoadImage(imgName);
	img = cvCreateImage(cvSize(bigImg->width/5, bigImg->height/5), bigImg->depth, bigImg->nChannels);
	cvResize(bigImg, img);
	pStorage = cvCreateMemStorage(0);
	pCascade = (CvHaarClassifierCascade*) 
		cvLoad("d:/program files/opencv231/data/haarCascades/haarcascade_frontalface_alt_tree.xml",
			0, 0, 0);
	pFaceRectSeq = cvHaarDetectObjects(img, pCascade, pStorage, 1.1, 3, 
			CV_HAAR_DO_CANNY_PRUNING, cvSize(40,40));

	cvNamedWindow("face");
	for (int i=0; i<(pFaceRectSeq?pFaceRectSeq->total:0); ++i) {
		CvRect* r = (CvRect*) cvGetSeqElem(pFaceRectSeq, i);
		CvPoint pt1 = {r->x, r->y};
		CvPoint pt2 = {r->x + r->width, r->y + r->height};
		cvRectangle(img, pt1, pt2, CV_RGB(0,255,0),3,4,0);
	}

	cvShowImage("face", img);

	cvReleaseImage(&img);
	cvReleaseHaarClassifierCascade(&pCascade);
	cvReleaseMemStorage(&pStorage);
}

void callbackButton(int state, void* userData) {
	char* cp = reinterpret_cast<char*>(userData);
	if (NULL != cp)
	{
		cout << cp << endl;
	}
	cout << state << " " << userData << endl;
}

void tryButton(void) {
	int val = 0;

	namedWindow("myWindow", CV_WINDOW_FREERATIO );
	createTrackbar("myTrack", "myWindow", &val, 255);
	createButton("button1", callbackButton, "button1", CV_PUSH_BUTTON);
	createButton("button2", callbackButton, NULL, CV_CHECKBOX);
	createButton("button3", callbackButton, "button3", CV_RADIOBOX);
	createButton("button4", callbackButton, "button4", CV_RADIOBOX);
}

void on_opengl(void* param)
{
	glLoadIdentity();

	glTranslated(0.0, 0.0, -1.0);

	glRotatef( 55, 1, 0, 0 );
	glRotatef( 45, 0, 1, 0 );
	glRotatef( 0, 0, 0, 1 );

	static const int coords[6][4][3] = {
		{ { +1, -1, -1 }, { -1, -1, -1 }, { -1, +1, -1 }, { +1, +1, -1 } },
		{ { +1, +1, -1 }, { -1, +1, -1 }, { -1, +1, +1 }, { +1, +1, +1 } },
		{ { +1, -1, +1 }, { +1, -1, -1 }, { +1, +1, -1 }, { +1, +1, +1 } },
		{ { -1, -1, -1 }, { -1, -1, +1 }, { -1, +1, +1 }, { -1, +1, -1 } },
		{ { +1, -1, +1 }, { -1, -1, +1 }, { -1, -1, -1 }, { +1, -1, -1 } },
		{ { -1, -1, +1 }, { +1, -1, +1 }, { +1, +1, +1 }, { -1, +1, +1 } }
	};

	for (int i = 0; i < 6; ++i) {
		glColor3ub( i*20, 100+i*10, i*42 );
		glBegin(GL_QUADS);
		for (int j = 0; j < 4; ++j) {
			glVertex3d(0.2 * coords[i][j][0], 0.2 * coords[i][j][1], 0.2 * coords[i][j][2]);
		}
		glEnd();
	}
}

void tryOpenglCallback(void) {
	namedWindow("myWind");
	createOpenGLCallback("myWind", on_opengl);
}

void tryBinaryFile(void) {
	FILE* file;
	fopen_s(&file, "depthDataRec.txt", "rb");
	int width, height;
	fscanf_s(file, " %d %d ", &width, &height);
	short* pixels = new short[width*height];
	int fct = 0;
	while (!feof(file)) {
		++fct;
		fread(pixels, sizeof(short), width*height, file);
		for (int i=0; i<10; ++i)
			cout << pixels[i] << " ";
		cout << endl;
	}
	cout << "frame: " << fct << endl;
	fclose(file);
}

void tryTLD(void) {
// 	IplImage* img1 = cvLoadImage(imgName, 0);
// 	IplImage* img2 = cvLoadImage(img2Name, 0);
// 	CvMat* mat = cvLoadImageM(imgName);
// 	cout << mat << endl;
// 	cvShowImage("img1", img1);
// 	cvShowImage("img2", img2);
// 
// 	CvMat* pyr1 = cvCreateMat(img1->height, img1->width, CV_8UC1);
// 	CvMat* pyr2 = cvCreateMat(img2->height, img2->width, CV_8UC1);
// 
// 	cvCalcOpticalFlowPyrLK( img1, img2, pyr1, pyr2, 
// 		points[0], points[1], nPts, cvSize(win_size,win_size), Level, status, 0, 
// 		cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), CV_LKFLOW_INITIAL_GUESSES);
}

void tryLargeFile(void) {
	char *mega = new char[1024*1024];
// 	memset(mega, 0, 1024*1024);
// 	for (int i=0; i<1024*1024; ++i)
// 		mega[i] = i%256;
// 	FILE* fout;
// 	fopen_s(&fout, "testlargefile.txt", "wb");
// 	for (int i=0; i<4*1024; ++i)
// 		fwrite(mega, 1, 1024*1024, fout);
// 	char tmp[10];
// 	for (int i=0; i<10; ++i)
// 		tmp[i] = 78;
// 	fwrite(tmp, 1, 10, fout);
// 	fclose(fout);

// 	FILE* fin;
// 	fopen_s(&fin, "testlargefile.txt", "rb");
// 	for (int i=0; i<4*1024; ++i)
// 		fread_s(mega, 1024*1024, 1, 1024*1024, fin);
// 	for (int i=1024*1024-10; i<1024*1024; ++i)
// 		printf("%d ", mega[i]);
// 	printf("\n");
// 	size_t tmpSize = fread_s(mega, 1024*1024, 1, 1024*1024, fin);
// 	cout << "tmpSize: " << tmpSize << endl;
// 	for (size_t i = 0; i < tmpSize; ++i)
// 		printf("%d ", mega[i]);
// 	printf("\n");
// 	fclose(fin);

	ifstream fin2("testlargefile.txt", ifstream::in | ifstream::binary);
	fin2.read(mega, 1024*1024);

	cvNamedWindow("hello world");
}

void tryOpticalFlow(void) {
	// Load two images and allocate other structures
	IplImage* imgA = cvLoadImage(imgName, CV_LOAD_IMAGE_GRAYSCALE);
	IplImage* imgB = cvLoadImage(img2Name, CV_LOAD_IMAGE_GRAYSCALE);

	CvSize img_sz = cvGetSize( imgA );
	int win_size = 15;

	IplImage* imgC = cvLoadImage("3.jpg", CV_LOAD_IMAGE_UNCHANGED);

	// Get the features for tracking
	IplImage* eig_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );
	IplImage* tmp_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );

	const int MAX_CORNERS = 50;
	int corner_count = MAX_CORNERS;
	CvPoint2D32f* cornersA = new CvPoint2D32f[ MAX_CORNERS ];

	cvGoodFeaturesToTrack( imgA, eig_image, tmp_image, cornersA, &corner_count,
		0.05, 5.0, 0, 3, 0, 0.04 );

	cvFindCornerSubPix( imgA, cornersA, corner_count, cvSize( win_size, win_size ),
		cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );

	// Call Lucas Kanade algorithm
	char features_found[ MAX_CORNERS ];
	float feature_errors[ MAX_CORNERS ];

	CvSize pyr_sz = cvSize( imgA->width+8, imgB->height/3 );

	IplImage* pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
	IplImage* pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );

	CvPoint2D32f* cornersB = new CvPoint2D32f[ MAX_CORNERS ];

	cvCalcOpticalFlowPyrLK( imgA, imgB, pyrA, pyrB, cornersA, cornersB, corner_count, 
		cvSize( win_size, win_size ), 5, features_found, feature_errors,
		cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3 ), 0 );

	// Make an image of the results

	for( int i=0; i<MAX_CORNERS; ++i ){
		if (features_found[i] == 0)
			continue;
		printf("Error is %f\n", feature_errors[i]);
		CvPoint p0 = cvPoint( cvRound( cornersA[i].x ), cvRound( cornersA[i].y ) );
		CvPoint p1 = cvPoint( cvRound( cornersB[i].x ), cvRound( cornersB[i].y ) );
		cvLine( imgC, p0, p1, CV_RGB(255,0,0), 2 );
	}

	cvNamedWindow( "ImageA", 0 );
	cvNamedWindow( "ImageB", 0 );
	cvNamedWindow( "LKpyr_OpticalFlow", 0 );

	cvShowImage( "ImageA", imgA );
	cvShowImage( "ImageB", imgB );
	cvShowImage( "LKpyr_OpticalFlow", imgC );
}

void tryKalman(void) {
	// Initialize Kalman filter object, window, number generator, etc
	cvNamedWindow( "Kalman", 1 );
	CvRandState rng;
	cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );

	IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
	CvKalman* kalman = cvCreateKalman( 2, 1, 0 );

	// State is phi, delta_phi - angle and angular velocity
	// Initialize with random guess
	CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 );
	cvRandSetRange( &rng, 0, 0.1, 0 );
	rng.disttype = CV_RAND_NORMAL;
	cvRand( &rng, x_k );

	// Process noise
	CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );

	// Measurements, only one parameter for angle
	CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 );
	cvZero( z_k );

	// Transition matrix F describes model parameters at and k and k+1
	const float F[] = { 1, 1, 0, 1 };
	memcpy( kalman->transition_matrix->data.fl, F, sizeof(F));

	// Initialize other Kalman parameters
	cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );
	cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );
	cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );
	cvSetIdentity( kalman->error_cov_post, cvRealScalar(1) );

	// Choose random initial state
	cvRand( &rng, kalman->state_post );

// 	x_k->data.fl[0] = kalman->state_post->data.fl[0];
// 	x_k->data.fl[1] = kalman->state_post->data.fl[1];
	// Make colors
	CvScalar yellow = CV_RGB(255,255,0);
	CvScalar white = CV_RGB(255,255,255);
	CvScalar red = CV_RGB(255,0,0);

	while( 1 ){
		cout << cvGet2D(x_k, 0, 0).val[0] << " " << cvGetReal2D(x_k, 1, 0) << endl;
		cout << cvGetReal2D(kalman->state_post, 0, 0) << " " << cvGetReal2D(kalman->state_post, 1, 0) << endl;
		// Predict point position
		const CvMat* y_k = cvKalmanPredict( kalman, 0 );
		cout << cvGetReal2D(y_k, 0, 0) << " " << cvGetReal2D(y_k, 1, 0) << endl;
		// Generate Measurement (z_k)
		cvRandSetRange( &rng, 0, sqrt( kalman->measurement_noise_cov->data.fl[0] ), 0 );			
		cvRand( &rng, z_k );
		cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );

		// Plot Points
		cvZero( img );
		// Yellow is observed state
		cvCircle( img, 
			cvPoint( cvRound(img->width/2 + img->width/3*cos(z_k->data.fl[0])),
			cvRound( img->height/2 - img->width/3*sin(z_k->data.fl[0])) ), 
			4, yellow );
		// White is the predicted state via the filter
		cvCircle( img, 
			cvPoint( cvRound(img->width/2 + img->width/3*cos(y_k->data.fl[0])),
			cvRound( img->height/2 - img->width/3*sin(y_k->data.fl[0])) ), 
			4, white, 2 );
		// Red is the real state
		cvCircle( img, 
			cvPoint( cvRound(img->width/2 + img->width/3*cos(x_k->data.fl[0])),
			cvRound( img->height/2 - img->width/3*sin(x_k->data.fl[0])) ),
			4, red );
		cvShowImage( "Kalman", img );

		// Adjust Kalman filter state
		cvKalmanCorrect( kalman, z_k );

		// Apply the transition matrix F and apply "process noise" w_k
		cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );
		cvRand( &rng, w_k );
		cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k );

		// Exit on esc key
		if( cvWaitKey( 100 ) == 27 ) 
			break;
	}
}

void tryGetSubRect(void) {
	CvMat* mat = cvCreateMat(3, 3, CV_8U);
	uchar* ptr = mat->data.ptr;
	for (int i=0; i<9; ++i, ++ptr)
		*ptr = i;
	CvMat* sub = cvCreateMatHeader(1, 1, CV_8U);

	cvGetSubRect(mat, sub, cvRect(0,0,2,2));
	*(sub->data.ptr) = 100;
	int x;
	x = 1;
}

CvPoint idxOffset[100][1000];

class CircleData {
public:
	int x, y, z, c;
};

void tryFindCircle(void) {
	CvMat *bigImg = cvCreateMat(256,256,CV_8UC1);
	int circumference[100];

	uchar **bigImgArr = new uchar *[256];
	for (int i=0; i<256; ++i)
		bigImgArr[i] = cvPtr2D(bigImg, i, 0);
	
	for (int r=0; r<100; ++r) {
		cvZero(bigImg);
		cvCircle(bigImg, cvPoint(127,127), r, cvScalar(255));
		circumference[r] = cvCountNonZero(bigImg);
		int k = 0;
		for (int i=0; i<256; ++i)
			for (int j=0; j<256; ++j)
				if (bigImgArr[i][j] != 0) {
					idxOffset[r][k].x = i-127;
					idxOffset[r][k].y = j-127;
					++k;
				}
		cout << r << " " << circumference[r] << " " << k << endl;
	}
	delete[] bigImgArr;

	CvMat *img = cvLoadImageM(imgName);
	int w, h;
	w = img->width;
	h = img->height;

	CvMat *gray = cvCreateMat(h, w, CV_8UC1);
	cvCvtColor(img, gray, CV_BGR2GRAY);

	cvShowImage("gray", gray);
	cvWaitKey(0);

	uchar **grayArr = new uchar *[h];
	for (int i=0; i<h; ++i)
		grayArr[i] = cvPtr2D(gray, i, 0);

	int lr, ur;
	lr = 18;
	ur = 23;
	int offset = 0;
	CircleData circles[1000];
	int circleIdx = 0;

	int debug=0;
	int maxCnt = -1;

	cout << w << " " << h << endl;

	for (int i=60; i<h-60; ++i) {
		for (int j=60; j<w-60; ++j) {
			int cnt[100];
			for (int k=lr-offset; k<ur+offset; ++k) {
				cnt[k] = 0;
				for (int t=0; t<circumference[k]; ++t) {
					int a, b;
					a = i+idxOffset[k][t].x;
					b = j+idxOffset[k][t].y;
					if (grayArr[a][b] > 10) {
						++cnt[k];
					}
				}
				if (cnt[k] > maxCnt) maxCnt = cnt[k];
			}
			for (int k=lr; k<ur; ++k) {
				if (cnt[k]>=0.5*circumference[k]) {
					circles[circleIdx].x = i;
					circles[circleIdx].y = j;
					circles[circleIdx].z = k;
					circles[circleIdx].c = cnt[k];
					++circleIdx;
				}
			}
		}
	}

	cout << maxCnt << endl;

	cout << "total : " << circleIdx << endl;
	CvMat *img1 = cvCloneMat(img);

	for (int i=0; i<circleIdx; ++i) {
		cout << circles[i].x << " " << circles[i].y << " " << circles[i].z << " "
			<< circles[i].c << " " << circles[i].c*1.0/circumference[circles[i].z] << endl;
		cvCopy(img1, img);
		cvCircle(img, cvPoint(circles[i].y, circles[i].x), circles[i].z, cvScalar(255, 0, 0));
		char name[100];
		sprintf(name, "img%d", i);
		cvShowImage(name, img);
	}
}

void tryCalibration(void) {
	CvMat *imgList[10];
	CvMat *img2List[10];

	char name[100];
	int pairNum = 5;
	for (int i=0; i<pairNum; ++i) {
		sprintf(name, "g%d.jpg", i+1);
		imgList[i] = cvLoadImageM(name);
		cvFlip(imgList[i], NULL, 1);
		sprintf(name, "c%d.jpg", i+1);
		img2List[i] = cvLoadImageM(name);
	}

	CvMat *drawImg = cvCloneMat(imgList[0]);

	CvPoint2D32f corners[100];
	CvPoint2D32f corners2[100];

	CvPoint2D32f objpts[10][100];
	CvPoint2D32f imgpts[10][100];
	CvPoint2D32f img2pts[10][100];
	int ptscnt[10];

// 	cvNamedWindow("img");
// 	cvNamedWindow("img2");
// 	cvMoveWindow("img", 10,10);
// 	cvMoveWindow("img2", 10+640+50, 10);

// 	for (int pairI = 0; pairI < pairNum; ++pairI) {
// 		CvMat *img;
// 		CvMat *img2;
// 		img = imgList[pairI];
// 		img2 = img2List[pairI];
// 		ptscnt[pairI] = 0;
// 		cvShowImage("img", img);
// 		cvShowImage("img2", img2);
// 		cvWaitKey(10);
// 
// 		int c=0;
// 		for (int i=4; i<10; ++i)
// 			for (int j=i; j<10; ++j) {
// 				CvSize patternSize = cvSize(i,j);
// 				int res = cvFindChessboardCorners(img, patternSize, corners, &c, 1+8);
// 				cout << pairI << " " << i << " " << j << " " << res << endl;
// 				cout << c << endl;
// 				if (res == 0) continue;
// 				res = cvFindChessboardCorners(img2, patternSize, corners2, &c, 1+8);
// 				cout << i << " " << j << " " << res << endl;
// 				cout << c << endl;
// 				if (res == 0) continue;
// 
// 				cvCopy(img, drawImg);
// 				cvDrawChessboardCorners(drawImg, patternSize, corners, c, res);
// 				cvShowImage("img", drawImg);
// 				cvCopy(img2, drawImg);
// 				cvDrawChessboardCorners(drawImg, patternSize, corners2, c, res);
// 				cvShowImage("img2", drawImg);
// 
// 				int ch = cvWaitKey(0);
// 
// 				if (ch == 13) {
// 					cout << "save it..." << endl;
// 					ptscnt[pairI] = c;
// 					for (int a=0; a<j; ++a) {
// 						for (int b=0; b<i; ++b) {
// 							objpts[pairI][a*i+b].x = b;
// 							objpts[pairI][a*i+b].y = a;
// 						}
// 					}
// 					for (int a=0; a<c; ++a) {
// 						imgpts[pairI][a] = corners[a];
// 						img2pts[pairI][a] = corners2[a];
// 					}
// 				}
// 			}
// 	}
// 
// 	int totalC = 0;
// 	for (int i=0; i<pairNum; ++i)
// 		totalC += ptscnt[i];
// 
	CvMat *objPts;
	CvMat *imgPts;
	CvMat *img2Pts;
	CvMat *nPts;
// 
// 	objPts = cvCreateMat(totalC, 3, CV_32FC1);
// 	imgPts = cvCreateMat(totalC, 2, CV_32FC1);
// 	img2Pts = cvCloneMat(imgPts);
// 	nPts = cvCreateMat(pairNum, 1, CV_32SC1);
// 
// 	float *objfp = objPts->data.fl;
// 	float *imgfp = imgPts->data.fl;
// 	float *img2fp = img2Pts->data.fl;
// 	for (int i=0; i<pairNum; ++i) {
// 		for (int j=0; j<ptscnt[i]; ++j, objfp+=3, imgfp+=2, img2fp+=2) {
// 			*objfp = objpts[i][j].x;
// 			*(objfp+1) = objpts[i][j].y;
// 			*(objfp+2) = 0;
// 
// 			*imgfp = imgpts[i][j].x;
// 			*(imgfp+1) = imgpts[i][j].y;
// 
// 			*img2fp = img2pts[i][j].x;
// 			*(img2fp+1) = img2pts[i][j].y;
// 		}
// 		cvSetReal1D(nPts, i, ptscnt[i]);
// 	}
// 
// 
// 	cvSave("objPtsMat.xml", objPts);
// 	cvSave("imgPtsMat.xml", imgPts);
// 	cvSave("img2PtsMat.xml", img2Pts);
// 	cvSave("nPtsMat.xml", nPts);
// 	exit(0);

	objPts = (CvMat *)cvLoad("objPtsMat.xml");
	imgPts = (CvMat *)cvLoad("imgPtsMat.xml");
	img2Pts = (CvMat *)cvLoad("img2PtsMat.xml");
	nPts = (CvMat *)cvLoad("nPtsMat.xml");

	CvMat *intriMat = cvCreateMat(3,3,CV_32FC1);
	CvMat *intriMat2 = cvCloneMat(intriMat);
	CvMat *distortMat = cvCreateMat(5,1,CV_32FC1);
	CvMat *distortMat2 = cvCloneMat(distortMat);
	CvMat *rVec = cvCreateMat(5, 3, CV_32FC1);
	CvMat *r2Vec = cvCloneMat(rVec);
	CvMat *tVec = cvCreateMat(5, 3, CV_32FC1);
	CvMat *t2Vec = cvCloneMat(tVec);

	double rep_err;
	rep_err = cvCalibrateCamera2(objPts, imgPts, nPts, cvGetSize(imgList[0]),
		intriMat, distortMat, rVec, tVec);
	cout << "rep_err: " << rep_err << endl;
	rep_err = cvCalibrateCamera2(objPts, img2Pts, nPts, cvGetSize(img2List[1]),
		intriMat2, distortMat2, r2Vec, t2Vec);
	cout << "rep_err: " << rep_err << endl;
	cvPrintMat(intriMat);
	cvPrintMat(distortMat);
	cvPrintMat(intriMat2);
	cvPrintMat(distortMat2);
	
	{
		double fx,fy,cx,cy;
		fx = cvGetReal2D(intriMat, 0, 0);
		fy = cvGetReal2D(intriMat, 1, 1);
		cx = cvGetReal2D(intriMat, 0, 2);
		cy = cvGetReal2D(intriMat, 1, 2);

		double x, y;
		x = cvGetReal2D(imgPts, 0, 0);
		y = cvGetReal2D(imgPts, 0, 1);
		cout << "img pts: " << x << " " << y << endl;
		double cmx,cmy;
		cmx = (x-cx)/fx;
		cmy = (y-cy)/fy;
		cout << cmx << " " << cmy << endl;
		Mat imgg1;
		imgg1 = imread("g1.jpg");
		flip(imgg1, imgg1, 1);
		circle(imgg1, cvPoint(x, y), 5, cvScalar(0,255,0));
// 		imshow("debug", imgg1);
		cvEzPrintMat(rVec);
		cvEzPrintMat(tVec);
		Mat R1, T1, tmpMat;
		tmpMat = Mat(rVec);
		Rodrigues(tmpMat.row(3), R1);
		tmpMat = Mat(tVec);
		transpose(tmpMat.row(3), T1);

		Mat R2, T2;
		tmpMat = Mat(r2Vec);
		Rodrigues(tmpMat.row(3), R2);
		tmpMat = Mat(t2Vec);
		transpose(tmpMat.row(3), T2);


		Mat R, T;
		invert(R1, R);
		T = -R2*R*T1 + T2;
		R = R2*R;

// 		CvMat *cvR = cvCreateMat(3,3,CV_32FC1);
// 		CvMat *cvT = cvCreateMat(3,1,CV_32FC1);
// 		CvMat *cvE = cvCreateMat(3,3,CV_64FC1);
// 		CvMat *cvF = cvCreateMat(3,3,CV_64FC1);
// 
// 		cvStereoCalibrate(objPts, imgPts, img2Pts, nPts, 
// 			intriMat, distortMat, 
// 			intriMat2, distortMat2,
// 			cvSize(imgList[0]->width, imgList[0]->height), cvR, cvT, cvE, cvF);
// 		R = Mat(cvR);
// 		T = Mat(cvT);

		ezPrintMat(R);
		ezPrintMat(T);

		double k1,k2,p1,p2,k3;
		k1 = cvGetReal1D(distortMat2, 0);
		k2 = cvGetReal1D(distortMat2, 1);
		k3 = cvGetReal1D(distortMat2, 4);
		p1 = cvGetReal1D(distortMat2, 2);
		p2 = cvGetReal1D(distortMat2, 3);
		

		for (int i=0; i<1; ++i) {
			Mat P;
			Mat objP;
			Point3_<float> augP;
			augP.x = cvGetReal2D(imgPts, i, 0);
			augP.y = cvGetReal2D(imgPts, i, 1);
			augP.z = 1;
			P = Mat(augP);
			ezPrintMat(P);
			transpose(Mat(objPts).row(i), objP);
			objP = R1*objP + T1;

			invert(Mat(intriMat), tmpMat);

			ezPrintMat(tmpMat);
			
			ezPrintMat(tmpMat*P);
			P = tmpMat*P;
			ezPrintMat(objP.at<float>(2) * P);
			P = objP.at<float>(2) * P;
			ezPrintMat(R*P+T);
			P = R*P+T;

			transpose(Mat(objPts).row(i), objP);
			objP = R2*objP + T2;
			ezPrintMat(objP);

// 			P = objP;
			ezPrintMat(P/P.at<float>(2));
			P = P/P.at<float>(2);


			double x,y,r;
			x = P.at<float>(0);
			y = P.at<float>(1);
			r = x*x+y*y;
			P.at<float>(0) = x*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p1*x*y + p2*(r+2*x*x);
			P.at<float>(1) = y*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p2*x*y + p1*(r+2*y*y);

			tmpMat = Mat(intriMat2);
			ezPrintMat(tmpMat*P);
			tmpMat = Mat(img2Pts);
			ezPrintMat(tmpMat.row(i));
		}


// 		for (int i=0; i<5; ++i) {
// 			tmpMat = Mat(objPts);
// 			Mat P;
// 			transpose(tmpMat.row(i), P);
// // 			ezPrintMat(P);
// 
// // 			ezPrintMat(R*P + T);
// 			P = R1*P + T1;
// // 			ezPrintMat(P/P.at<float>(2));
// 			P = P/P.at<float>(2);
// 
// 			double x,y,r;
// 			x = P.at<float>(0);
// 			y = P.at<float>(1);
// 			r = x*x+y*y;
// 			P.at<float>(0) = x*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p1*x*y + p2*(r+2*x*x);
// 			P.at<float>(1) = y*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p2*x*y + p1*(r+2*y*y);
// 
// 			tmpMat = Mat(intriMat);
// 			ezPrintMat(tmpMat*P);
// 			tmpMat = Mat(imgPts);
// 			ezPrintMat(tmpMat.row(i));
// 		}


		//cvWaitKey(0);
		exit(0);

	}



	


	int prevN = 1;
	int totPrev = 0;
	CvMat *newn = cvCreateMat(prevN , 1, CV_32SC1);
	for (int i=0; i<prevN; ++i) {
		cvSetReal2D(newn, i, 0, cvGetReal2D(nPts, i, 0));
		totPrev += cvGetReal2D(nPts, i, 0);
	}
	CvMat *newobj = cvCreateMat(totPrev, 3, CV_32FC1);
	CvMat *newimg = cvCreateMat(totPrev, 2, CV_32FC1);
	CvMat *newimg2 = cvCloneMat(newimg);

	for (int i=0; i<newobj->rows; ++i) {
		for (int j=0; j<3; ++j) {
			cvSetReal2D(newobj, i, j, cvGetReal2D(objPts, i, j));
		}
		for (int j=0; j<2; ++j) {
			cvSetReal2D(newimg, i, j, cvGetReal2D(imgPts, i, j));
			cvSetReal2D(newimg2, i, j, cvGetReal2D(img2Pts, i, j));
		}
	}

	rVec = cvCreateMat(3, 1, CV_32FC1);
	r2Vec = cvCreateMat(3, 1, CV_32FC1);
	tVec = cvCreateMat(3, 1, CV_32FC1);
	t2Vec = cvCreateMat(3, 1, CV_32FC1);
	cvFindExtrinsicCameraParams2(newobj, newimg, intriMat, distortMat, rVec, tVec);
 	cvFindExtrinsicCameraParams2(newobj, newimg2, intriMat2, distortMat2, r2Vec, t2Vec);

	cout << "extri param: " << endl;
	cvPrintMat(rVec);
	cvPrintMat(tVec);
	cvPrintMat(r2Vec);
	cvPrintMat(t2Vec);

	CvMat *resImgPts = cvCloneMat(imgPts);

	cvProjectPoints2(objPts, rVec, tVec, intriMat, distortMat, resImgPts);
	for (int i=0; i<5; ++i) {
		cout << "obj: " << cvGetReal2D(objPts, i, 0) << " " << cvGetReal2D(objPts, i, 1)
			<< " " << cvGetReal2D(objPts, i, 2) << "   " <<
			"img: " << cvGetReal2D(imgPts, i, 0) <<
			" " << cvGetReal2D(imgPts, i, 1) << "   " << 
			cvGetReal2D(resImgPts, i, 0) << " " <<
			cvGetReal2D(resImgPts, i, 1) << endl;
	}

	cvProjectPoints2(objPts, r2Vec, t2Vec, intriMat2, distortMat2, resImgPts);
	for (int i=0; i<5; ++i) {
		cout << "obj: " << cvGetReal2D(objPts, i, 0) << " " << cvGetReal2D(objPts, i, 1)
			<< " " << cvGetReal2D(objPts, i, 2) << "   " <<
			"img2: " << cvGetReal2D(img2Pts, i, 0) <<
			" " << cvGetReal2D(img2Pts, i, 1) << "   " << 
			cvGetReal2D(resImgPts, i, 0) << " " <<
			cvGetReal2D(resImgPts, i, 1) << endl;
	}



	CvMat *R = cvCreateMat(3,3,CV_64FC1);
	CvMat *T = cvCreateMat(3,1,CV_64FC1);
	CvMat *E = cvCreateMat(3,3,CV_64FC1);
	CvMat *F = cvCreateMat(3,3,CV_64FC1);

	cvStereoCalibrate(objPts, imgPts, img2Pts, nPts, 
		intriMat, distortMat, 
		intriMat2, distortMat2,
		cvSize(imgList[0]->width, imgList[0]->height), R, T, E, F);

	CvMat *lines = cvCreateMat(newimg->rows, 3, CV_64FC1);

	cvComputeCorrespondEpilines(newimg, 1, F, lines);
	cout << "R" << endl;
	cvPrintMat(R);
	cout << "T" << endl;
	cvPrintMat(T);
	cout << "E" << endl;
	cvPrintMat(E);
	cout << "F" << endl;
	cvPrintMat(F);
	cout << "lines: " << endl;
	for (int i=0; i<5; ++i) {
		for (int j=0; j<3; ++j)
			cout << cvGetReal2D(lines, i, j) << " ";
		cout<<endl;
	}


	cvFindFundamentalMat(newimg, newimg2, F);
	cout << "F2" << endl;
	cvPrintMat(F);
// 	cvFindFundamentalMat(imgPts, img2Pts, F);
// 	cout << "F3" << endl;
// 	cvPrintMat(F);
	cvComputeCorrespondEpilines(newimg, 1, F, lines);
	cout << "lines2: " << endl;
	for (int i=0; i<5; ++i) {
		for (int j=0; j<3; ++j)
			cout << cvGetReal2D(lines, i, j) << " ";
		cout<<endl;
	}

	CvMat *R1 = cvCreateMat(3,3,CV_64FC1);
	CvMat *R2 = cvCloneMat(R1);
	CvMat *P1 = cvCreateMat(3,4,CV_64FC1);
	CvMat *P2 = cvCloneMat(P1);

	CvMat *Q = cvCreateMat(4,4,CV_64FC1);

	cvStereoRectify(intriMat, intriMat2, distortMat, distortMat2,
		cvGetSize(imgList[0]), R, T, R1, R2, P1, P2, Q);

	cout << "after rectify" << endl;
	cout << "R" << endl;
	cvPrintMat(R);
	cout << "T" << endl;
	cvPrintMat(T);
	cout << "R1" << endl;
	cvPrintMat(R1);
	cout << "P1" << endl;
	cvPrintMat(P1);
	cout << "R2" << endl;
	cvPrintMat(R2);
	cout << "P2" << endl;
	cvPrintMat(P2);

	CvMat *newIntriMat = cvCreateMatHeader(3,3,CV_64FC1);
	cvGetSubRect(intriMat, newIntriMat, cvRect(0,0,3,3));

	CvMat *map1=cvCreateMat(imgList[0]->rows, imgList[0]->cols, CV_32FC1);
	CvMat *map2=cvCloneMat(map1);

	CvMat *remapmat = cvCloneMat(imgList[0]);
	CvMat *remapmat2 = cvCloneMat(img2List[0]);


	for (int i=0; i<3; ++i) {
		cvInitUndistortRectifyMap(intriMat, distortMat, R1, P1, map1, map2);
		cvRemap(imgList[i], remapmat, map1, map2);
		cvShowImage("remap", remapmat);

		cvInitUndistortRectifyMap(intriMat2, distortMat2, R2, P2, map1, map2);
		cvRemap(img2List[i], remapmat2, map1, map2);
		cvShowImage("remap2", remapmat2);
		cvShowImage("img", imgList[i]);
		cvShowImage("img2", img2List[i]);
		cvWaitKey(0);
	}



	CvMat *disparity = cvCreateMat(imgList[0]->rows, imgList[0]->cols,CV_32FC1);
	
	CvMat *gray1 = cvCloneMat(disparity);
	cvCvtColor(cvConvertMatType(remapmat,CV_32FC3), gray1, CV_BGR2GRAY);
	CvMat *gray2 = cvCloneMat(gray1);
	cvCvtColor(cvConvertMatType(remapmat2,CV_32FC3), gray2, CV_BGR2GRAY);

	cvSub(gray1, gray2, disparity);
	CvMat *_3dimage = cvCreateMat(imgList[0]->rows,imgList[0]->cols,CV_32FC3);
	cvConvert(_3dimage, _3dimage, 1./512, 0);
	cvReprojectImageTo3D(disparity, _3dimage, Q);

	cvShowImage("3d", _3dimage);


	cvShowImage("img", imgList[0]);
	for (int i=0; i<5; ++i) {
		for (int j=0; j<5; ++j) {
			cout << "map: " << cvGetReal2D(map1, i, j) <<
				" " << cvGetReal2D(map2, i, j) << endl;
		}
	}

	for (int i=0; i<5; ++i) {
		cout << "obj: " << cvGetReal2D(objPts, i, 0) << 
			" " << cvGetReal2D(objPts, i, 1) << 
			" " << cvGetReal2D(objPts, i, 2) << endl;
		cout << "img: " << cvGetReal2D(imgPts, i, 0) <<
			" " << cvGetReal2D(imgPts, i, 1) << endl;
		cout << "img2: " << cvGetReal2D(img2Pts, i, 0) <<
			" " << cvGetReal2D(img2Pts, i, 1) << endl;
	}


// 	cvReleaseMat(&objPts);
//	cvStereoCalibrate()
}

void tryGeometric(void) {
	CvMat *objPts, *imgPts, *img2Pts, *nPts;
	objPts = (CvMat *)cvLoad("objPtsMat.xml");
	imgPts = (CvMat *)cvLoad("imgPtsMat.xml");
	img2Pts = (CvMat *)cvLoad("img2PtsMat.xml");
	nPts = (CvMat *)cvLoad("nPtsMat.xml");

	CvMat *resMat;
	resMat = cvCreateMat(3,3,CV_32FC1);

	CvPoint2D32f list1[100], list2[100];
	int ptsIds[] = {0, 5, 53, 48};
	for (int i=0; i<4; ++i) {
		list1[i].x = cvGetReal2D(imgPts, ptsIds[i], 0);
		list1[i].y = cvGetReal2D(imgPts, ptsIds[i], 1);
		list2[i].x = cvGetReal2D(img2Pts, ptsIds[i], 0);
		list2[i].y = cvGetReal2D(img2Pts, ptsIds[i], 1);
	}

	int man1x[] = {127,91,376,425};
	int man1y[] = {92,296,393,113};
	int man2x[] = {206,229,555,506};
	int man2y[] = {164,362,309,125};

// 	for (int i=0; i<4; ++i) {
// 		list1[i].x = man1x[i];
// 		list1[i].y = man1y[i];
// 		list2[i].x = man2x[i];
// 		list2[i].y = man2y[i];
// 	}


	cout << "list1: " << endl;
	for (int i=0; i<4; ++i) {
		cout << list1[i].x << " " << list1[i].y << endl;
	}
	cout << "list2: " << endl;
	for (int i=0; i<4; ++i) {
		cout << list2[i].x << " " << list2[i].y << endl;
	}

	cvGetPerspectiveTransform(list1, list2, resMat);
// 	cvGetAffineTransform(list1, list2, resMat);

	CvMat *img;
	CvMat *img2;

	img = cvLoadImageM("g2.jpg");
	cvFlip(img, NULL, 1);
	img2 = cvLoadImageM("c2.jpg");

	cvShowImage("img", img);
	cvShowImage("img2", img2);

	CvMat *dst;
	dst = cvCloneMat(img);
// 	cvWarpAffine(img, dst, resMat);
	cvWarpPerspective(img, dst, resMat);
	cvShowImage("dst", dst);

}

void tryFundementalMat(void) {
	CvMat *imgList[3][20];
	int WIDTH = 640;
	int HEIGHT = 480;

	DepthReader dReader("./d/depth_data.dat", "./d/depth_meta.txt");
	ColorReader cReader("./d/color_data.dat", "./d/color_meta.txt");
	GrayReader gReader("./d/gray_data.dat", "./d/gray_meta.txt");

	FILE *inF;
	inF = fopen("./d/landmark.txt", "r");
	CvPoint cptrs[20], gptrs[20], dptrs[20];
	for (int i=0; i<13; ++i) {
		fscanf(inF, "%d %d %d %d", &cptrs[i].x, &cptrs[i].y, &gptrs[i].x, &gptrs[i].y);
	}
	fclose(inF);

	cvNamedWindow("depth");
	cvNamedWindow("color");
	cvNamedWindow("gray");
	cvMoveWindow("color", 10,10);
	cvMoveWindow("gray", 10+640+10,10);
	cvMoveWindow("depth", 10,10);

	CvMat *dFrame = cvCreateMat(HEIGHT, WIDTH, CV_16S);
	CvMat *cFrame = cvCreateMat(HEIGHT, WIDTH, CV_8UC3);
	CvMat *gFrame = cvCreateMat(HEIGHT, WIDTH, CV_8U);

	CvMat *ucharFrame = cvConvertMatType(dFrame, CV_8UC1);
	CvMat *map1=NULL;
	CvMat *map2=NULL;
	char name[100];

	for (int i=0; i<13; ++i) {
		cout << i << endl;
		dReader.getNextFrame(dFrame->data.s);
		cReader.getNextFrame(cFrame->data.ptr);
		gReader.getNextFrame(gFrame->data.ptr);
		if (i == 4) continue;


		sprintf(name, "./d/dcmap1mat%d.xml", i+1);
		map1 = (CvMat *)cvLoad(name);
		sprintf(name, "./d/dcmap2mat%d.xml", i+1);
		map2 = (CvMat *)cvLoad(name);

		int x, y;
		bool isBreak = false;
		for (int j=0; j<HEIGHT; ++j) {
			for (int k=0; k<WIDTH; ++k) {
				x = cvGetReal2D(map2, j, k);
				y = cvGetReal2D(map1, j, k);
				if (abs(x-cptrs[i].x)<3 && abs(y-cptrs[i].y)<3) {
					cout << cptrs[i].x << " " << cptrs[i].y << " " << x << " " << y << " " << 
						cvGetReal2D(dFrame, j, k) << endl;
					x = k;
					y = j;
					cout << "yes" << x << " " << y << endl;
					isBreak = true;
					break;
				}
			}
			if (isBreak) break;
		}

		cvConvertScale(dFrame, ucharFrame, 255./16067, 0);
		cvFlip(gFrame, gFrame, 1);

		cvCircle(cFrame, cptrs[i], 3, cvScalar(0,0,255));
		cvCircle(gFrame, gptrs[i], 3, cvScalar(0));
		cvCircle(ucharFrame, cvPoint(x,y), 3, cvScalar(127));
		dptrs[i].x = x;
		dptrs[i].y = y;


// 		cvShowImage("gray", gFrame);
// 		cvShowImage("color", cFrame);
//  		cvShowImage("depth", ucharFrame);
// 		cvWaitKey(10);
	}

	CvMat *dptrM = cvCreateMat(12,2,CV_32FC1);
	CvMat *gptrM = cvCloneMat(dptrM);
	CvMat *cptrM = cvCloneMat(dptrM);


	int idx = 0;
	for (int i=0; i<13; ++i) {
		if (i==4) continue;
		cvSetReal2D(dptrM, idx, 0, dptrs[i].x);
		cvSetReal2D(dptrM, idx, 1, dptrs[i].y);
		cvSetReal2D(gptrM, idx, 0, gptrs[i].x);
		cvSetReal2D(gptrM, idx, 1, gptrs[i].y);
		cvSetReal2D(cptrM, idx, 0, cptrs[i].x);
		cvSetReal2D(cptrM, idx, 1, cptrs[i].y);
		++idx;
	}

	CvMat *fund1 = cvCreateMat(3,3,CV_32FC1);
	CvMat *fund2 = cvCloneMat(fund1);
	cvFindFundamentalMat(cptrM, gptrM, fund1);
	cvFindFundamentalMat(dptrM, gptrM, fund2);

	cout << "fund1" << endl;
	cvPrintMat(fund1);
	cout << "fund2" << endl;
	cvPrintMat(fund2);
}

void tryCali2(void) {
	const int WIDTH = 640;
	const int HEIGHT = 480;

	int saveIdx = 0;

	vector< vector<Point3f> > objPts;
	vector< vector<Point2f> > imgPts;

	char mode = 0;
	cout << "choose mode: (c)color or (g)gray or (0) load both" << endl;
	cin >> mode;
	if (mode == 'd' || mode == 'c') {
		KinectDepthColorWrapper kdcWrapper;
		kdcWrapper.init(WIDTH, HEIGHT);
		CvMat *depthRes = NULL, *colorRes = NULL;

		long *colorCoordinates = new long[WIDTH*HEIGHT*2];
		Mat mapped(HEIGHT, WIDTH, CV_16S);

		while (true) {
			for (int i=0; i<HEIGHT; ++i) {
				for (int j=0; j<WIDTH; ++j) {
					colorCoordinates[2*(i*WIDTH+j)]=j;
					colorCoordinates[2*(i*WIDTH+j)+1]=i;
					mapped.at<short>(i,j) = 0;
				}
			}
			kdcWrapper.retrieve(depthRes, colorRes, colorCoordinates);
			Mat depth, color;
			depth = Mat(depthRes);
			color = Mat(colorRes);


			for (int i=0; i<HEIGHT; ++i) {
				for (int j=0; j<WIDTH; ++j) {
					int x, y;
					x = colorCoordinates[2*(i*WIDTH+j)];
					y = colorCoordinates[2*(i*WIDTH+j)+1];
					if (0<=x && x<WIDTH && 0<=y && y<HEIGHT && depth.at<short>(i,j) > 100) {
						//					memcpy(mapped.ptr<uchar>(i,j), color.ptr<uchar>(y,x), sizeof(uchar)*3);
						mapped.at<short>(y,x) = depth.at<short>(i,j);
					}
					else {
						//					memset(mapped.ptr<uchar>(i,j), 0, sizeof(uchar)*3);
					}
				}
			}

			imshow("depth", depth);
			imshow("color", color);
			imshow("mapped", mapped);
			char ch = cvWaitKey(50);
			if (ch == 27) break;
			if (ch == 13) {
				bool finish = false;

				for (int i=9; i>=5; --i) {
					if (finish) break;
					for (int j=i; j>=5; --j) {
						Size patternSize(i,j);
						vector<Point2f> corners;
						int res = findChessboardCorners(color, patternSize, corners, 1+8);
						cout << i << " " << j << " " << res << endl;
						if (res == 0) continue;

						Mat drawImg = color;
						drawChessboardCorners(drawImg, patternSize, corners, res);
						imshow("corner", drawImg);

						ch = cvWaitKey(0);

						if (ch == 13) {
							cout << "save it..." << endl;
							finish = true;
							char prefix[100];
							sprintf(prefix, "data%d", saveIdx++);
							string prefixStr(prefix);
							imwrite(prefixStr+"_c.jpg", color);
							FileStorage fs(prefixStr+"_depthValue.yml", FileStorage::WRITE);
							fs << "mapped" << mapped;
							fs << "depth" << depth;
							fs.release();
							imwrite(prefixStr+"_d.jpg", mapped);
							imwrite(prefixStr+"_depthOriginalImage.jpg", depth);

							vector<Point3f> objpts;

							for (int a=0; a<j; ++a) {
								for (int b=0; b<i; ++b) {
									objpts.push_back(Point3f(b,a,0));
								}
							}
							objPts.push_back(objpts);
							imgPts.push_back(corners);

							break;
						}
					}
				}
			}
		}

		cout << "now calibration" << endl;
		Mat intriMat, distortMat;
		vector<Mat> rvecs, tvecs;
		calibrateCamera(objPts, imgPts, Size(HEIGHT, WIDTH), intriMat, distortMat, rvecs, tvecs);
		ezPrintMat(intriMat);
		ezPrintMat(distortMat);


		FileStorage fs("calibrate_color_result.yml", FileStorage::WRITE);

		vector<float> flatVec;
		for (int i=0; i<objPts.size(); ++i) {
			for (int j=0; j<objPts[i].size(); ++j) {
				flatVec.push_back(objPts[i][j].x);
				flatVec.push_back(objPts[i][j].y);
				flatVec.push_back(objPts[i][j].z);
			}
			flatVec.push_back(-1);
			flatVec.push_back(-1);
			flatVec.push_back(-1);
		}
		Mat objPtsMat(flatVec.size()/3, 3, CV_32FC1, flatVec.data());
		fs << "objPtsMat" << objPtsMat;

		flatVec.clear();
		for (int i=0; i<imgPts.size(); ++i) {
			for (int j=0; j<imgPts[i].size(); ++j) {
				flatVec.push_back(imgPts[i][j].x);
				flatVec.push_back(imgPts[i][j].y);
				flatVec.push_back(1);
			}
			flatVec.push_back(-1);
			flatVec.push_back(-1);
			flatVec.push_back(-1);
		}
		Mat imgPtsMat(flatVec.size()/3, 3, CV_32FC1, flatVec.data());
		fs << "imgPtsMat" << imgPtsMat;

		fs << "rvecs" << "[";
		for (int i=0; i<rvecs.size(); ++i)
			fs << rvecs[i];
		fs << "]";

		fs << "tvecs" << "[";
		for (int i=0; i<tvecs.size(); ++i)
			fs << tvecs[i];
		fs << "]";

		fs << "intriMat" << intriMat;
		fs << "distortMat" << distortMat;
		fs.release();
	}
	else if (mode == 'g') {
		VideoCapture capture(CV_CAP_DSHOW+1);
		capture.set(CV_CAP_PROP_FRAME_WIDTH, WIDTH);
		capture.set(CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);

		while (true) {
			Mat gray;
			capture.read(gray);
			flip(gray, gray, 1);
			imshow("gray", gray);
			char ch = cvWaitKey(50);
			if (ch == 27) break;
			if (ch == 13) {
				bool finish = false;

				for (int i=9; i>=5; --i) {
					if (finish) break;
					for (int j=i; j>=5; --j) {
						Size patternSize(i,j);
						vector<Point2f> corners;
						int res = findChessboardCorners(gray, patternSize, corners, 1+8);
						cout << i << " " << j << " " << res << endl;
						if (res == 0) continue;

						Mat drawImg = gray;
						drawChessboardCorners(drawImg, patternSize, corners, res);
						imshow("corner", drawImg);

						ch = cvWaitKey(0);

						if (ch == 13) {
							cout << "save it..." << endl;
							finish = true;
							char prefix[100];
							sprintf(prefix, "data%d", saveIdx++);
							string prefixStr(prefix);
							imwrite(prefixStr+"_g.jpg", gray);

							vector<Point3f> objpts;

							for (int a=0; a<j; ++a) {
								for (int b=0; b<i; ++b) {
									objpts.push_back(Point3f(b,a,0));
								}
							}
							objPts.push_back(objpts);
							imgPts.push_back(corners);

							break;
						}
					}
				}
			}
		}

		cout << "now calibration" << endl;
		Mat intriMat, distortMat;
		vector<Mat> rvecs, tvecs;
		calibrateCamera(objPts, imgPts, Size(HEIGHT, WIDTH), intriMat, distortMat, rvecs, tvecs);
		ezPrintMat(intriMat);
		ezPrintMat(distortMat);


		FileStorage fs("calibrate_gray_result.yml", FileStorage::WRITE);

		vector<float> flatVec;
		for (int i=0; i<objPts.size(); ++i) {
			for (int j=0; j<objPts[i].size(); ++j) {
				flatVec.push_back(objPts[i][j].x);
				flatVec.push_back(objPts[i][j].y);
				flatVec.push_back(objPts[i][j].z);
			}
			flatVec.push_back(-1);
			flatVec.push_back(-1);
			flatVec.push_back(-1);
		}
		Mat objPtsMat(flatVec.size()/3, 3, CV_32FC1, flatVec.data());
		fs << "objPtsMat" << objPtsMat;

		fs << "Size" << Size(HEIGHT, WIDTH);

		flatVec.clear();
		for (int i=0; i<imgPts.size(); ++i) {
			for (int j=0; j<imgPts[i].size(); ++j) {
				flatVec.push_back(imgPts[i][j].x);
				flatVec.push_back(imgPts[i][j].y);
				flatVec.push_back(1);
			}
			flatVec.push_back(-1);
			flatVec.push_back(-1);
			flatVec.push_back(-1);
		}
		Mat imgPtsMat(flatVec.size()/3, 3, CV_32FC1, flatVec.data());
		fs << "imgPtsMat" << imgPtsMat;

		fs << "rvecs" << "[";
		for (int i=0; i<rvecs.size(); ++i)
			fs << rvecs[i];
		fs << "]";

		fs << "tvecs" << "[";
		for (int i=0; i<tvecs.size(); ++i)
			fs << tvecs[i];
		fs << "]";

		fs << "intriMat" << intriMat;
		fs << "distortMat" << distortMat;
		fs.release();
	}
	else if (mode == '0') {

		FileStorage fs;
		FileNode vecs;
		Mat intriMatC, intriMatG;
		Mat distortMatC, distortMatG;
		vector<Mat> rvecsC, rvecsG, tvecsC, tvecsG;


		fs.open("calibrate_color_result.yml", FileStorage::READ);
		fs["intriMat"] >> intriMatC;
		fs["distortMat"] >> distortMatC;
		vecs = fs["rvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			rvecsC.push_back(tmp);
		}
		vecs = fs["tvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			tvecsC.push_back(tmp);
		}
		fs.release();


		fs.open("calibrate_gray_result.yml", FileStorage::READ);
		fs["intriMat"] >> intriMatG;
		fs["distortMat"] >> distortMatG;
		vecs = fs["rvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			rvecsG.push_back(tmp);
		}
		vecs = fs["tvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			tvecsG.push_back(tmp);
		}
		fs.release();

		{

			Mat R1, T1;
			Rodrigues(rvecsC[0], R1);
			T1 = tvecsC[0];
			ezPrintMat(R1);
			ezPrintMat(T1);

			Mat R2, T2;
			Rodrigues(rvecsG[0], R2);
			T2 = tvecsG[0];
			ezPrintMat(R2);
			ezPrintMat(T2);

			Mat R, T;
			R = R1.inv();
			T = -R2*R*T1 + T2;
			R = R2*R;

			ezPrintMat(R);
			ezPrintMat(T);

			double k1,k2,p1,p2,k3;
			k1 = distortMatG.at<double>(0);
			k2 = distortMatG.at<double>(1);
			k3 = distortMatG.at<double>(4);
			p1 = distortMatG.at<double>(2);
			p2 = distortMatG.at<double>(3);

			//reading from device
			Mat mapped(HEIGHT, WIDTH, CV_8UC3);
			Mat dcMapped(HEIGHT, WIDTH, CV_16S);
			KinectDepthColorWrapper kdcWrapper;
			kdcWrapper.init(WIDTH, HEIGHT);
			VideoCapture capture(CV_CAP_DSHOW+1);
			capture.set(CV_CAP_PROP_FRAME_WIDTH, WIDTH);
			capture.set(CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);

			long *colorCoordinates = new long[WIDTH*HEIGHT*2];
			Mat c0, d0, g0;
			CvMat *depthRes = NULL, *colorRes = NULL;

			while (true) {
				for (int i=0; i<HEIGHT; ++i) {
					for (int j=0; j<WIDTH; ++j) {
						colorCoordinates[2*(i*WIDTH+j)]=j;
						colorCoordinates[2*(i*WIDTH+j)+1]=i;
						memset(mapped.ptr<uchar>(i,j), 0, sizeof(uchar)*3);
						dcMapped.at<short>(i,j) = 0;
					}
				}
				kdcWrapper.retrieve(depthRes, colorRes, colorCoordinates);
				capture.read(g0);

				d0 = Mat(depthRes);
				c0 = Mat(colorRes);

				imshow("depth", d0);
				imshow("color", c0);
				imshow("gray", g0);
				char ch = cvWaitKey(0);
				if (ch != 13) continue;

				for (int i=0; i<HEIGHT; ++i) {
					for (int j=0; j<WIDTH; ++j) {
						int d2cX, d2cY;
						d2cX = colorCoordinates[2*(i*WIDTH+j)];
						d2cY = colorCoordinates[2*(i*WIDTH+j)+1];
						if (0<=d2cX && d2cX<WIDTH && 0<=d2cY && d2cY<HEIGHT && d0.at<short>(i,j) > 100) {
							short dval = d0.at<short>(i,j);
							dcMapped.at<short>(d2cY,d2cX) = dval;

							Mat P = (Mat_<double>(3,1) << d2cX,d2cY,1);
// 							ezPrintMat(P);
// 							ezPrintMat(intriMatC.inv()*P);
							P = intriMatC.inv()*P;
							P = dval/10.0 * P;
// 							ezPrintMat(R*P+T);
							P = R*P+T;

// 							ezPrintMat(P/P.at<double>(2));
							P = P/P.at<double>(2);

							double x,y,r;
							x = P.at<double>(0);
							y = P.at<double>(1);
							r = x*x+y*y;
							P.at<double>(0) = x*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p1*x*y + p2*(r+2*x*x);
							P.at<double>(1) = y*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p2*x*y + p1*(r+2*y*y);

// 							ezPrintMat(intriMatG*P);
							P = intriMatG*P;

							int px, py;
							px = cvRound(P.at<double>(0));
							py = cvRound(P.at<double>(1));
							if (0<=px && px<WIDTH && 0<=py && py<HEIGHT) {
								memcpy(mapped.ptr<uchar>(py,px), c0.ptr<uchar>(d2cY, d2cX), sizeof(uchar)*3);
							}
						}
					}
				}

				FileStorage fs("test_data.yml", FileStorage::WRITE);
				fs << "dcMapped" << dcMapped;
				fs << "c0" << c0;
				fs << "g0" << g0;
				fs << "d0" << d0;
				fs.release();
				
				imshow("gray", g0);
				imshow("mapped", mapped);
				ch = cvWaitKey(0);
				if (ch == 27) break;
			}
		}
	}

}


int main(int argc, char* argv[])
{
// 	imgName = argv[1];
// 	img2Name = argv[2];
	imgName = "color.jpg";
	img2Name = "gray.jpg";
// 	cout << imgName << " " << img2Name << endl;

//  	showImage();
//	tryMouseEvent();
//	tryHarris();
//	tryCascade();
// 	tryVideo();
// 	tryButton();
// 	tryOpenglCallback();
//	tryBinaryFile();
// 	tryTLD();
//	tryLargeFile();
// 	tryOpticalFlow();
//	tryKalman();
//  	tryGetSubRect();
//	tryHist();
//	tryFindCircle();
	tryCalibration();
// 	tryGeometric();
// 	tryCali2();
    cvWaitKey(0);
    cvDestroyAllWindows();
	return 0;
}
